#include "PowerFlowCalculation.h"


gsl_matrix* GradsHeisenMatrix(Node m_Node[],gsl_matrix *m_G,gsl_matrix *m_B,int Node_Num,int GenCount)
{
    gsl_matrix* h1=gsl_matrix_alloc(GenCount,GenCount);
    gsl_matrix* h2=gsl_matrix_alloc(GenCount,Node_Num);
    gsl_matrix* h3=gsl_matrix_alloc(GenCount,Node_Num-1);
    gsl_matrix* h4=gsl_matrix_alloc(Node_Num,GenCount);
    gsl_matrix* h5=gsl_matrix_alloc(Node_Num,Node_Num);
    gsl_matrix* h6=gsl_matrix_alloc(Node_Num,Node_Num-1);
    gsl_matrix* h7=gsl_matrix_alloc(Node_Num-1,GenCount);
    gsl_matrix* h8=gsl_matrix_alloc(Node_Num-1,Node_Num);
    gsl_matrix* h9=gsl_matrix_alloc(Node_Num-1,Node_Num-1);
    gsl_matrix* h11=gsl_matrix_alloc(Node_Num,1);
    gsl_matrix* h22=gsl_matrix_alloc(Node_Num-1,1);
    gsl_matrix* Hession_obj=gsl_matrix_alloc(Node_Num*2-1+GenCount,Node_Num*2-1+GenCount);

    
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            if(i==j)
            {
                gsl_matrix_set(h5,i,i,2*gsl_matrix_get(m_G,i,i));
            }
            else
            {
                double f = m_Node[i].Angle - m_Node[j].Angle;
                gsl_matrix_set(h5,i,j,2*gsl_matrix_get(m_G,i,j)*cos(f));
            }
        }
    }
    

    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            double f = m_Node[i].Angle - m_Node[j].Angle;
            gsl_matrix_set(h11,i,0,gsl_matrix_get(h11,i,0)-2*m_Node[j].Vol*gsl_matrix_get(m_G,i,j)*sin(f));
            if(i!=Node_Num-1 && i!=j){
                gsl_matrix_set(h22,i,0,gsl_matrix_get(h22,i,0)-2*m_Node[i].Vol*m_Node[j].Vol*gsl_matrix_get(m_G,i,j)*cos(f));
            }
        }
    }
    
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            if(i==j)
            {
                gsl_matrix_set(h6,i,i,gsl_matrix_get(h11,i,0));
            }
            else if(i!=j)
            {
                double f = m_Node[i].Angle - m_Node[j].Angle;
                gsl_matrix_set(h6,i,j,2*m_Node[j].Vol*gsl_matrix_get(m_G,i,j)*sin(f));

            }
        }
    }
    
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            if(i==j)
            {
                gsl_matrix_set(h8,i,i,gsl_matrix_get(h11,i,0));
            }
            else if(i!=j)
            {
                double f = m_Node[i].Angle - m_Node[j].Angle;
                gsl_matrix_set(h8,i,j,-2*m_Node[j].Vol*gsl_matrix_get(m_G,i,j)*sin(f));

            }
        }
    }
    
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            if(i==j)
            {
                gsl_matrix_set(h9,i,i,gsl_matrix_get(h22,i,0));
            }
            else if(i!=j)
            {
                double f = m_Node[i].Angle - m_Node[j].Angle;
                gsl_matrix_set(h9,i,j,2*m_Node[j].Vol*m_Node[i].Vol*gsl_matrix_get(m_G,i,j)*cos(f));

            }
        }
    }
    // PRINT_MATRIX(h8)
    // return Hession_obj;
    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession_obj,i,j,gsl_matrix_get(h1,i,j));
        }
    }
    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession_obj,i,GenCount+j,gsl_matrix_get(h2,i,j));
        }
    }
    for(int i=0;i<GenCount;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession_obj,i,GenCount+Node_Num+j,gsl_matrix_get(h3,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession_obj,GenCount+i,j,gsl_matrix_get(h4,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession_obj,GenCount+i,j+GenCount,gsl_matrix_get(h5,i,j));
            //printf("%f\n",gsl_matrix_get(h5,i,j));
        }
    }
//PRINT_MATRIX(Hession_obj)
    for(int i=0;i<Node_Num;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession_obj,GenCount+i,j+GenCount+Node_Num,gsl_matrix_get(h6,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<GenCount;j++)
        {
            gsl_matrix_set(Hession_obj,GenCount+Node_Num+i,j,gsl_matrix_get(h7,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num;j++)
        {
            gsl_matrix_set(Hession_obj,GenCount+Node_Num+i,j+GenCount,gsl_matrix_get(h8,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++)
    {
        for(int j=0;j<Node_Num-1;j++)
        {
            gsl_matrix_set(Hession_obj,GenCount+Node_Num+i,j+GenCount+Node_Num,gsl_matrix_get(h9,i,j));
        }
    }


    
    return Hession_obj;



}